#include "max30102.h"
#include "esp_err.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "dsps_biquad_gen.h"
#include "dsps_biquad.h"
#include <string.h>

#define DEFAULT_DATA_CACHE_SIZE 128

#define REG_INTR_STATUS_1 0x00
#define REG_INTR_STATUS_2 0x01
#define REG_INTR_ENABLE_1 0x02
#define REG_INTR_ENABLE_2 0x03
#define REG_FIFO_WR_PTR 0x04
#define REG_OVF_COUNTER 0x05
#define REG_FIFO_RD_PTR 0x06
#define REG_FIFO_DATA 0x07
#define REG_FIFO_CONFIG 0x08
#define REG_MODE_CONFIG 0x09
#define REG_SPO2_CONFIG 0x0A
#define REG_LED1_PA 0x0C
#define REG_LED2_PA 0x0D
#define REG_PILOT_PA 0x10
#define REG_MULTI_LED_CTRL1 0x11
#define REG_MULTI_LED_CTRL2 0x12
#define REG_TEMP_INTR 0x1F
#define REG_TEMP_FRAC 0x20
#define REG_TEMP_CONFIG 0x21
#define REG_PROX_INT_THRESH 0x30
#define REG_REV_ID 0xFE
#define REG_PART_ID 0xFF

struct max30102_t
{
    i2c_master_dev_handle_t iic_dev_handle;
    size_t data_cache_size;
    float *ir, *red;
    float *ir_filtered, *red_filtered;
    uint16_t bpm;
    float spo2;
    float iir_coeffs[5];
    float ir_w[2], red_w[2];
    float q_factor;
    float f;
};

// esp_err_t dsps_biquad_gen_bpf_f32(float *coeffs, float f, float qFactor)

static esp_err_t max30102_write_reg_(max30102_handle_t max30102, uint8_t addr, uint8_t data)
{
    uint8_t write_reg_buffer[2] = {addr, data};
    return i2c_master_transmit(max30102->iic_dev_handle, write_reg_buffer, 2, 1000);
}

static esp_err_t max30102_read_reg_(max30102_handle_t max30102, uint8_t addr, uint8_t *data, size_t size)
{
    return i2c_master_transmit_receive(max30102->iic_dev_handle, &addr, 1, data, size, 1000);
}

static max30102_handle_t max30102_create_(i2c_master_bus_handle_t i2c_bus)
{
    i2c_device_config_t dev_cfg = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = 0x57,
        .scl_speed_hz = 80000,
    };
    max30102_handle_t max30102 = malloc(sizeof(struct max30102_t));
    memset(max30102, 0, sizeof(struct max30102_t));
    i2c_master_bus_add_device(i2c_bus, &dev_cfg, &(max30102->iic_dev_handle));
    max30102->data_cache_size = DEFAULT_DATA_CACHE_SIZE;
    max30102->ir = malloc(sizeof(float) * max30102->data_cache_size);
    max30102->red = malloc(sizeof(float) * max30102->data_cache_size);
    max30102->ir_filtered = malloc(sizeof(float) * max30102->data_cache_size);
    max30102->red_filtered = malloc(sizeof(float) * max30102->data_cache_size);
    return max30102;
}

static esp_err_t max30102_get_status_(max30102_handle_t max30102, uint8_t *status)
{
    return max30102_read_reg_(max30102, REG_INTR_STATUS_1, status, 1);
}

// 初始化max30102
max30102_handle_t max30102_init(i2c_master_bus_handle_t i2c_bus)
{
    max30102_handle_t max30102;
    uint8_t uch_dummy;
    // 创建max30102设备
    max30102 = max30102_create_(i2c_bus);

    dsps_biquad_gen_bpf_f32(max30102->iir_coeffs, 0.14, 0.7);

    // 重置max30102
    max30102_write_reg_(max30102, REG_MODE_CONFIG, 0x40);
    vTaskDelay(10 / portTICK_PERIOD_MS);

    ESP_ERROR_CHECK(max30102_read_reg_(max30102, REG_INTR_STATUS_1, &uch_dummy, 1));
    ESP_ERROR_CHECK(max30102_read_reg_(max30102, REG_INTR_STATUS_2, &uch_dummy, 1));

    // 不使用中断
    ESP_ERROR_CHECK(max30102_write_reg_(max30102, REG_INTR_ENABLE_1, 0x40));
    ESP_ERROR_CHECK(max30102_write_reg_(max30102, REG_INTR_ENABLE_2, 0x00));

    ESP_ERROR_CHECK(max30102_write_reg_(max30102, REG_FIFO_WR_PTR, 0x00)); // FIFO_WR_PTR[4:0]
    ESP_ERROR_CHECK(max30102_write_reg_(max30102, REG_OVF_COUNTER, 0x00)); // OVF_COUNTER[4:0]
    ESP_ERROR_CHECK(max30102_write_reg_(max30102, REG_FIFO_RD_PTR, 0x00)); // FIFO_RD_PTR[4:0]

    ESP_ERROR_CHECK(max30102_write_reg_(max30102, REG_FIFO_CONFIG, 0x8f)); // 16 avg- fifo rollover=false, fifo almost full = 17
    ESP_ERROR_CHECK(max30102_write_reg_(max30102, REG_SPO2_CONFIG, 0x2f)); // 400HZ- SPO2_ADC range = 4096nA,  LED pulseWidth (400uS)

    ESP_ERROR_CHECK(max30102_write_reg_(max30102, REG_LED1_PA, 0x24)); // Choose value for ~ 7mA for LED1
    ESP_ERROR_CHECK(max30102_write_reg_(max30102, REG_LED2_PA, 0x24)); // Choose value for ~ 7mA for LED2

    ESP_ERROR_CHECK(max30102_write_reg_(max30102, REG_PILOT_PA, 0x7f)); // Choose value for ~ 25mA for Pilot LED

    return max30102;
}

// 读取max30102的FIFO数据
static esp_err_t max30102_read_fifo_(max30102_handle_t max30102, float *pun_red_led, float *pun_ir_led)
{
    uint8_t receive_data[6];
    uint32_t data[2];
    uint8_t status;
    do
    {
        vTaskDelay(17);
        ESP_ERROR_CHECK(max30102_get_status_(max30102, &status));
    } while ((status & 0x40) == 0);

    ESP_ERROR_CHECK(max30102_read_reg_(max30102, REG_FIFO_DATA, receive_data, 6));

    data[0] = ((receive_data[0] << 16 | receive_data[1] << 8 | receive_data[2]) & 0x03ffff);
    data[1] = ((receive_data[3] << 16 | receive_data[4] << 8 | receive_data[5]) & 0x03ffff);
    *pun_red_led = data[0];
    *pun_ir_led = data[1];
    return ESP_OK;
}

// 开始测量
esp_err_t max30102_start(max30102_handle_t max30102)
{
    esp_err_t err = max30102_write_reg_(max30102, REG_MODE_CONFIG, 0x03);
    if (err != ESP_OK)
    {
        printf("max30102_start error\n");
    }
    max30102->bpm = 0;
    max30102->spo2 = 0;
    for (int i = 0; i < max30102->data_cache_size; i++)
    {
        max30102_read_fifo_(max30102, max30102->red + i, max30102->ir + i);
    }
    dsps_biquad_f32(max30102->ir, max30102->ir_filtered, max30102->data_cache_size, max30102->iir_coeffs, max30102->ir_w);
    return ESP_OK;
}

int cmp_uint16(const void *a, const void *b)
{
    return *(uint16_t *)a - *(uint16_t *)b;
}

esp_err_t max30102_get_heart_rate(max30102_handle_t max30102)
{
    float ir_aver = 0;
    float ir_max = max30102->ir_filtered[0], ir_min = max30102->ir_filtered[0];
    uint16_t prev = 0;
    uint16_t bmp_buffer[32];
    uint16_t bmp = 0;
    uint8_t bmp_buffer_size = 0;
    // 参考线
    float guidline;
    int right = 1;
    int window_begin_index = -1, window_len = 0;
    uint16_t width = 3;

    for (int i = 0; i < max30102->data_cache_size; ++i)
    {
        ir_aver += max30102->ir_filtered[i];
        if (ir_max < max30102->ir_filtered[i])
            ir_max = max30102->ir_filtered[i];

        if (ir_min > max30102->ir_filtered[i])
            ir_min = max30102->ir_filtered[i];
    }
    ir_aver /= max30102->data_cache_size;
    guidline = (ir_aver + ir_min) / 2;

    for (int i = 1; i < max30102->data_cache_size; ++i)
    {
        if (max30102->ir_filtered[i] <= guidline && max30102->ir_filtered[i - 1] > guidline)
        {
            if (prev != 0)
            {
                bmp_buffer[bmp_buffer_size++] = i - prev;
            }
            prev = i;
        }
    }

    qsort(bmp_buffer, bmp_buffer_size, sizeof(uint16_t), cmp_uint16);

    for (int i = 0; i < bmp_buffer_size && right < bmp_buffer_size; i++)
    {
        while (right < bmp_buffer_size && bmp_buffer[right] - bmp_buffer[i] < width)
        {
            right++;
        }
        if (right - i >= window_len)
        {
            window_len = right - i;
            window_begin_index = i;
        }
    }

    printf("bufer_size:%d window_begin_index:%d window_len:%d\n", bmp_buffer_size, window_begin_index, window_len);
    for (int i = window_begin_index; i < window_begin_index + window_len; i++)
    {
        bmp += bmp_buffer[i];
    }
    if (window_len != 0)
    {
        bmp = (bmp + window_len - 1) / window_len;
        max30102->bpm = 60 * 25 / bmp;
    }

    return ESP_OK;
}

int cmp(const void *a, const void *b)
{
    return *(float *)a - *(float *)b;
}

esp_err_t max30102_get_spo2(max30102_handle_t max30102)
{
    float *ir_input_data = max30102->ir;
    float *red_input_data = max30102->red;
    int max_index = -1, min_index = -1;
    float r;
    float spo2_buffer[32];
    float spo2 = 0;
    int window_begin_index = -1, window_len = 0;
    float width = 1.5;
    int right = 0;
    int spo2_buffer_size = 0;
    float pi_red, pi_ir;
    uint16_t cache_nums = max30102->data_cache_size;
    // 寻找局部最大值
    for (int i = 1; i < cache_nums - 1; i++)
    {
        while (i < cache_nums - 1)
        {
            if (ir_input_data[i] > ir_input_data[i - 1] && ir_input_data[i] > ir_input_data[i + 1] && red_input_data[i] > red_input_data[i - 1] && red_input_data[i] > red_input_data[i + 1])
            {
                max_index = i;
                break;
            }
            ++i;
        }

        while (i < cache_nums - 1)
        {
            if (ir_input_data[i] < ir_input_data[i - 1] && ir_input_data[i] < ir_input_data[i + 1] && red_input_data[i] < red_input_data[i - 1] && red_input_data[i] < red_input_data[i + 1])
            {
                min_index = i;
                break;
            }
            ++i;
        }
        pi_red = ((red_input_data[max_index] - red_input_data[min_index]) / red_input_data[min_index]);
        pi_ir = ((ir_input_data[max_index] - ir_input_data[min_index]) / ir_input_data[min_index]);
        r = pi_red / pi_ir;
        spo2 = ((-45.060) * r * r + 30.354 * r + 94.845);
        spo2_buffer[spo2_buffer_size++] = spo2;
        if (spo2_buffer_size == 32)
        {
            break;
        }
    }

    qsort(spo2_buffer, spo2_buffer_size, sizeof(float), cmp);

    for (int i = 0; i < spo2_buffer_size && right < spo2_buffer_size; i++)
    {
        while (right < spo2_buffer_size && spo2_buffer[right] - spo2_buffer[i] < width)
        {
            right++;
        }
        if (right - i >= window_len)
        {
            window_len = right - i;
            window_begin_index = i;
        }
    }

    spo2 = 0;

    for (int i = window_begin_index; i < window_begin_index + window_len; i++)
    {
        spo2 += spo2_buffer[i];
    }

    if (window_len != 0)
    {
        max30102->spo2 = spo2 / window_len;
    }
    return ESP_OK;
}

// 读取数据到缓存
esp_err_t max30102_read_data(max30102_handle_t max30102)
{
    for (size_t i = 0; i < max30102->data_cache_size; i++)
    {
        max30102_read_fifo_(max30102, max30102->red + i, max30102->ir + i);
    }
    dsps_biquad_gen_bpf0db_f32(max30102->iir_coeffs, 0.14, 0.7);
    dsps_biquad_f32(max30102->ir, max30102->ir_filtered, max30102->data_cache_size, max30102->iir_coeffs, max30102->ir_w);
    // dsps_biquad_f32(max30102->red, max30102->red_filtered, max30102->data_cache_size, max30102->iir_coeffs, max30102->red_w);
    max30102_get_heart_rate(max30102);
    max30102_get_spo2(max30102);

    return ESP_OK;
}

esp_err_t max30102_get_bmp_spo2(max30102_handle_t max30102, uint16_t *bpm, float *spo2)
{
    *bpm = max30102->bpm;
    *spo2 = max30102->spo2;
    return ESP_OK;
}

// 停止测量
esp_err_t max30102_stop(max30102_handle_t max30102)
{
    return max30102_write_reg_(max30102, REG_MODE_CONFIG, 0x00);
}
